#include <inc/mp.h>
#include <inc/cpu.h>
#include <inc/stdio.h>
#include <inc/string.h>
#include <inc/pmap.h>
#include <inc/apic.h>
#include <inc/x86.h>

#define BOOTP 0x2
#define KERN_BASE 0x0



uint32_t index = 0;
volatile struct cpu cpus[MAX_CPU];
uint32_t ioapicid;
uint64_t cpu_id = 0;

static uint32_t sum(uint8_t *p, uint32_t size)
{
    uint8_t totl = 0;
    uint32_t i;
    for(i = 0; i < size; i ++)	
	totl += p[i];
    return totl;
}

static void apic_mtrr_init()
{
    write_msr(0x230, 0xfee00000);
    write_msr(0x231, (((((uint64_t) 1)<<36 )- 1 ) & ~(1000 -1)) | 0x800);
    write_msr(0x232, 0xfec00000);
    write_msr(0x233, (((((uint64_t) 1)<<36) - 1 ) & ~(1000 -1)) | 0x800);
}

static struct mp_fptr* mp_search(uint64_t base, uint32_t limit)
{
    uint32_t offset = 0;
    struct mp_fptr *fptr;
    while(offset < limit)
    {
	fptr = (struct mp_fptr*)(base + offset);
	if(fptr->ft_sig[0] == '_' &&fptr->ft_sig[1] == 'M' && 
	   fptr->ft_sig[2] == 'P' && fptr->ft_sig[3] == '_')
	{
	    if(sum((uint8_t *)fptr, sizeof(struct mp_fptr)) == 0)
		return fptr;
	}
	offset += sizeof(struct mp_fptr);
    }
    return 0;
}

struct mp_fptr* mp_get_fptr(void)
{
    uint8_t *addr;
    uint16_t ebda;
    uint16_t base_boundary;
    struct mp_fptr *fptr;
    addr = (uint8_t *)0x400;
    ebda = (addr[0xf]<<8 | addr[0xe])<<4;
    base_boundary = (addr[0x14]<<16 | addr[0x13])*1024;
    /* 1st KB in the EBDA area */
    if((fptr = mp_search(ebda, 1024)))
	return fptr;
    /* last KB in the base memory */
    if((fptr = mp_search(base_boundary - 1024, 1024)))
	return fptr;
    /* last KB in the BIOS memeory */
    if((fptr = mp_search(0xf0000, 0x10000)))
	return fptr;
    return 0;
}


int mp_init()
{
    struct mp_fptr *fptr;
    struct mp_conf_header *mp_header;
    struct mp_processer *proc;
    struct mp_busentry *bus;
    struct mp_ioapic *ioapic;
    struct mp_iointr_assign *iointr;
    struct mp_locintr_assign *lintr;

    //apic_mtrr_init();
    //ioapic_tmp_map(0xfec00000);
    //ioapic_tmp_map(0xfec01000);

    //lapic_tmp_map(0xfee00000);
    cprintf("lapic id is 0x%x\n", read_lapic(0x20));
    fptr = mp_get_fptr();
    if(!fptr)
	return -1;
    if(!fptr->mp_feature[0])
	cprintf("mp table exists\n");

    mp_header = (struct mp_conf_header *)((uint64_t)  fptr->tb_addr + KERN_BASE);
    //	cprintf("extended table size is %u bytes\n", mp_header->ext_t_length);
    //	cprintf("the lapic mapped address is 0x%x\n", mp_header->lapicaddr);
    uint8_t *p = (uint8_t *)(mp_header + 1);
    uint8_t j = 0;
	
    for(; p < ((uint8_t *)mp_header + mp_header->base_t_length);)
    {
	switch(*p)
	{
	case PROC:
	    proc = (struct mp_processer *) p;
	    cprintf("lapic version 0x%x and lapic id is 0x%x\n",
		    proc->lapicver, proc->lapicid); 
	    cpus[j].lapicid = proc->lapicid;
	    cpus[j].cpuid = proc->lapicid;
	    cpus[j].nodeid = proc->lapicid;
	    if(proc->cpuflags & BOOTP)
		cpus[j].bootp = 1;
	    p += 20;
	    j ++;
	    continue;
	case BUS:
	    bus = (struct mp_busentry *) p;
	    p += 8;
	    continue;
	case IOAPIC:
	    ioapic = (struct mp_ioapic *) p;
	    cprintf("the ioapic %d mem mapped addr is 0x%x,enable/disalbed:%d\n", ioapic->ioapicid,
		    ioapic->mmioaipc_addr,ioapic->ioapicflag); 
	    ioapicid = ioapic->ioapicid;
	    p += 8;
	    continue;
	case LINTR:
	    lintr = (struct mp_locintr_assign *) p;
	    p += 8;
	    continue;
	case IOINTR:
	    iointr = (struct mp_iointr_assign *) p;
	    cprintf("the irq intr is for ioapic:%u, the line is %u\n",
		    iointr->desioapicid, iointr->desioapicintn); 
	    p += 8;
	    continue;
	default:
	    cprintf("error! Unkown mp conf type\n");
	    return -1;
	}
    }
    cprintf("the total cpu is %d\n", j);
	
    ioapic_init();
    if(fptr->mp_feature[1])
	cprintf("IMCR is present\n");
    uint32_t max_entry = read_ioapic(0x1);
    //	cprintf("max ioapic entries is 0x%x\n", max_entry);
    enable_lapic();
    apic_ipi(0x0, 132);
    for(uint32_t i = 0; i < j; i ++)
    {
	if(!cpus[i].bootp)
	{
	    cpu_id = (cpus[i].cpuid - 1 - 4)*4096;
	    boot_ap(0x40000, cpus[i].cpuid);
	    lock_cprintf("boot_ap here\n");
	    while(!cpus[i].booted)
		__asm__ __volatile__("pause"::);
	    if(cpus[i].cpuid == 5)
		break;
	}
    }
    cprintf("i'm return, total cpu is %d \n", j);

    return 0;
}


